/*需求：1.解析雷达数据

数据类型：
波特率 230400/

FA A0  BB 0B    E9 04 AC 03 0D 00   37 05 E2 04 0D 00   1E 05 97 04 0D 00   97 05 51 04 0D 00   0A 02 00 00 0D 14    A6 01 00 00 10 04   16  16 
FA A1  B1 0B   B5 06 00 00  11 08   5E 05 56 09 17 00   5A 07 00 00 1B 00   78 07 00 00 1F 00   B3 10 00 00 23 00    91 01 00 0A 0B 00   54  54 
头 索  速度     强度         距离   预留 	强度   距离 预留	强度   距离 预留    强度   距离 预留	 强度   距离 预留	  强度   距离 预留	  校验

解析流程

esp32一次读取82字节数据存储数组，从数据里面挑选FA验证开头，A0验证首位，验证校验和，取出6个距离数据

写一个esp32串口解析360度激光雷达的程序，
雷达数据为FA开头，第二位为A0-DB共60组数据，每组数据共计42字节，每组数据FA开头，第2字节A0为角度值(范围A0-DB共60组)，
第7/8字节为第一度距离（低位在前高位在后），
第13/14字节为第二度距离（低位在前高位在后），
第19/20字节为第三度距离（低位在前高位在后），
第25/26字节为第四度距离（低位在前高位在后），
第31/32字节为第五度距离（低位在前高位在后），
第37/38字节为第六角度距离（低位在前高位在后），
第41/42字节为校验和（低位在前高位在后）
计算校验和方法（校验和=0xFF减去所有数据相加）


*/

#include <HardwareSerial.h>

// 全局常量定义
const uint8_t PACKET_SIZE = 42;  // 完整数据包长度
const uint8_t MEASUREMENTS_PER_PACKET = 6;  // 每包测量值数量

HardwareSerial LidarSerial(2); 

void setup() {
  Serial.begin(230400);
  LidarSerial.begin(230400, SERIAL_8N1, 16, 17);
  Serial.println("Lidar on");
  LidarSerial.print("b");
  delay(2000);
  Serial.println("Lidar Parser Ready");
}

void loop() {
  static uint8_t buffer[PACKET_SIZE];
  static uint8_t readPos = 0;

  while (LidarSerial.available()) {
    uint8_t data = LidarSerial.read();

    if (readPos == 0 && data != 0xFA) continue;  // 同步头检测

    buffer[readPos++] = data;

    if (readPos >= PACKET_SIZE) {
      if (buffer[0] == 0xFA) {
        parsePacket(buffer);
      }
      readPos = 0;
    }
  }
}

void parsePacket(uint8_t* packet) {
  if (!validateAngle(packet[1])) {
    Serial.println("ERR: Invalid angle");
    return;
  }

  if (!validateChecksum(packet)) {
    Serial.println("ERR: Checksum failed");
    return;
  }

  processMeasurements(packet);
}

bool validateAngle(uint8_t angleByte) {
  return (angleByte >= 0xA0) && (angleByte <= 0xDB);
}

bool validateChecksum(uint8_t* packet) {
  uint32_t sum = 0;
  for (int i = 0; i < PACKET_SIZE - 2; i++) {
    sum += packet[i];
  }
  
  uint8_t expected = 0xFF - (sum & 0xFF);
  uint16_t received = (packet[PACKET_SIZE-2] << 8) | packet[PACKET_SIZE-1];

  return (received & 0xFF) == expected;
}

void processMeasurements(uint8_t* packet) 
{
  uint8_t angleIndex = packet[1] - 0xA0;
  float startAngle = angleIndex * 6.0f;

  uint16_t measurements[MEASUREMENTS_PER_PACKET];
  for (int i = 0; i < MEASUREMENTS_PER_PACKET; i++)
  {
    uint8_t offset = 6 + i * 6;
    measurements[i] = (packet[offset] ) | packet[offset + 1]<< 8;
  }

  // 调试输出（可根据需要修改格式）
  Serial.print("Sector ");
  Serial.print(angleIndex);
  Serial.print(" (");
  Serial.print(startAngle);
  Serial.print("°): ");
  
  for (int i = 0; i < MEASUREMENTS_PER_PACKET; i++) {
    Serial.print(measurements[i]);
    Serial.print("mm ");
    if (i == 2) Serial.print("| ");
  }
  Serial.println();
}